% Exemplo Livro Maciejowski
% A = [0.0211 0 0 0; 0.1062 0.4266 0 0; 0 0 0.2837 0; 0.1012 -0.6688 0.2893 0.4266];
% B = [0.6462 0.6462; 0.2800 0.2800; 1.5237 -0.7391; 0.9929 0.1507];
% C = [0 1 0 0; 0 0 0 1];

% Exemplo Apostila Anderson
% A = [0.7326 -0.1722; 0.08611 0.9909];
% B = [0.08611; 0.004528];
% C = [0 3];

% Tanques Interligados
% A = [0.9936 0; 0.006359 0.9936];
% B = [0.04783; 0.0001529];
% C = [0 1];

% Tanques Interligados My Version
% A = [-0.0656 0; 0.0656 -0.0656];
% B = [0.2964; 0];
% C = [0 1];
% Ts = 0.5:
% A = [0.9677 0; 0.03174 0.9677];
% B = [0.1458; 0.002378];
% C = [0 1];
% Ts = 0.1:
% A = [0.9935 0; 0.006517 0.9935];
% B = [0.02954 9.68e-005];
% C = [0 1];
% Ts = 0.2:
% A = [0.987 0; 0.01295 0.987];
% B = [0.05889 0.0003855];
% C = [0 1];
% Ts = 0.3:
% A = [0.9805 0; 0.0193 0.9805];
% B = [0.08805 0.0008636];
% C = [0 1];
% Ts = 0.4:
A = [0.9741 0; 0.02556 0.9741];
B = [0.117 0.001529];
C = [0 1];

cB = size(B, 2);
rC = size(C, 1);

Hw = 1;
Hp = 5;
Hu = 2;

simulationSteps = 500;

setPoint = 10 * ones(simulationSteps, rC);

Q = 1 * [zeros((Hw - 1) * rC, Hp * rC); zeros((Hp - Hw + 1) * rC, (Hw - 1) * rC) eye((Hp - Hw + 1) * rC)];
%R = 10 * [eye(Hu * cB) zeros(Hu * rC, (Hp - Hu) * rC); zeros((Hp - Hu) * cB, Hp * cB)];

%Q = 1 * eye((Hp - Hw + 1) * rC);
R = 0.8 * eye(Hu * cB);

constraintsESeed = [];
constraintsFSeed = [-1/3 -1; 1/3 -1];
constraintsGSeed = [-1 0 0; 1/50 0 -1; 0 -1 0; 0 1/10 -1];

constraintsE = generateConstraints(Hu, constraintsESeed);
constraintsF = generateConstraints(Hu, constraintsFSeed);
constraintsG = generateConstraints(Hp, constraintsGSeed);

clc;

% ssmpc(simulationSteps, Hp, Hu, Q, R, A, B, C, setPoint);
constrainedSsmpc(simulationSteps, Hp, Hu, Q, R, A, B, C, setPoint, constraintsE, constraintsF, constraintsG);